﻿using FarseerPhysics.Dynamics;
using FarseerPhysics.Factories;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using FarseerPhysics.DebugViews;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Dynamics.Joints;
using FarseerPhysics.Common;
using System.Collections.Generic;
using FarseerPhysics.Dynamics.Contacts;

namespace FarseerPhysics.HelloWorld
{
    /// <summary>
    /// This is the main type for your game
    /// </summary>
    public class Game1 : Game
    {
        private GraphicsDeviceManager _graphics;
        private SpriteBatch _batch;
        private KeyboardState _oldKeyState;
        private GamePadState _oldPadState;
        private SpriteFont _font;

        private World _world;
        private MyPlayerBody _body;
        private CpuBody _body2;
        private Body _circleBody;
        private Body _groundBody;

        private Texture2D _circleSprite;
        private Texture2D _groundSprite;

        // Simple camera controls
        private Matrix _view;
        private Vector2 _cameraPosition;
        private Vector2 _screenCenter;

        private DebugViewXNA _debugView;      

        private Body _trackSensor;
      
        const float kTrackHeight = 33;//27.78f;
        const float kTrackRadius = 8;//5.56f;
        const float kTrackWidth = 11f;
        const float kTackOffset = 5f;

        private Vector2 _groundPosition;

#if !XBOX360
        const string Text = "Press W A D to move the rider\n" +
                            "Press HOME to return to starting position\n";                            
#else
                const string Text = "Use left stick to move\n" +
                                    "Use right stick to move camera\n" +
                                    "Press A to jump\n";
#endif
        
        // Farseer expects objects to be scaled to MKS (meters, kilos, seconds)
        // 1 meters equals 64 pixels here
        // (Objects should be scaled to be between 0.1 and 10 meters in size)
        private const float MeterInPixels = 64f;

        public Game1()
        {
            _graphics = new GraphicsDeviceManager(this);
            _graphics.PreferredBackBufferWidth = 1680; //1024;//1680; // 800
            _graphics.PreferredBackBufferHeight = 1050; //768;//1050; // 480

            Content.RootDirectory = "Content";

            _world = new World(new Vector2(0, 0));

            _debugView = new DebugViewXNA(_world);
          
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            // create and configure the debug view
            _debugView = new DebugViewXNA(_world);
            _debugView.AppendFlags(DebugViewFlags.Shape);
            _debugView.DefaultShapeColor = Color.White;
            _debugView.SleepingShapeColor = Color.LightGray;
            _debugView.LoadContent(GraphicsDevice, Content);

            // Initialize camera controls
            _view = Matrix.Identity;
            _cameraPosition = Vector2.Zero;

            _screenCenter = new Vector2(_graphics.GraphicsDevice.Viewport.Width / 2f,
                                                _graphics.GraphicsDevice.Viewport.Height / 2f);

            _batch = new SpriteBatch(_graphics.GraphicsDevice);
            _font = Content.Load<SpriteFont>("font");

            // Load sprites
            _circleSprite = Content.Load<Texture2D>("circleSprite"); //  96px x 96px => 1.5m x 1.5m
            _groundSprite = Content.Load<Texture2D>("groundSprite"); // 512px x 64px =>   8m x 1m

            /* Circle */
            // Convert screen center from pixels to meters
            Vector2 circlePosition = (_screenCenter / MeterInPixels) + new Vector2(0, -1.5f);

            // Create the circle fixture
            _circleBody = BodyFactory.CreateCircle(_world, 96f / (2f * MeterInPixels), 1f, circlePosition);
            _circleBody.BodyType = BodyType.Dynamic;

            // Give it some bounce and friction
            _circleBody.Restitution = 0.3f;
            _circleBody.Friction = 0.5f;

            /* Ground */
            _groundPosition = (_screenCenter / MeterInPixels); /*+ new Vector2(0, 1.25f);*/

            _trackSensor = new Body(_world);
            //Vertices trackVerts = PolygonTools.CreateCapsule(3000f / MeterInPixels, 800f / MeterInPixels, 25);
            // create track
            Vertices trackVerts = PolygonTools.CreateCapsule(kTrackHeight + kTrackWidth + kTackOffset, kTrackRadius + (kTrackWidth / 2), 25);
            PolygonShape trackShape = new PolygonShape(trackVerts, 1);
            
            _trackSensor.CreateFixture(trackShape);
            _trackSensor.Rotation = MathHelper.ToRadians(90);
            _trackSensor.IsSensor = true;
            _trackSensor.Position = _groundPosition;

            // Create the ground fixture
            _groundBody = new Body(_world);            
            //Vertices verts = PolygonTools.CreateCapsule(2000f / MeterInPixels, 500f / MeterInPixels, 25);
            // create ground - grass
            Vertices verts = PolygonTools.CreateCapsule(kTrackHeight, kTrackRadius, 25);
            
            PolygonShape rectangleShape = new PolygonShape(verts, 1);
            _groundBody.CreateFixture(rectangleShape);
            _groundBody.Rotation = MathHelper.ToRadians(90);
            _groundBody.IsSensor = true;
            _groundBody.Restitution = 0.3f;
            _groundBody.Friction = 0.5f;
            _groundBody.Position = _groundPosition;

            //Vertices pathVerts = PolygonTools.CreateCapsule(kTrackHeight + 2f, kTrackRadius + 2f, 25);
            Vertices pathVerts = PolygonTools.CreateCapsule(kTrackHeight + (kTrackWidth + kTackOffset) / 2, kTrackRadius + ((kTrackWidth / 2) / 2), 25);
            pathVerts.Rotate(MathHelper.ToRadians(90));
            // move to ground position
            for (int i = 0; i < pathVerts.Count; ++i)
                pathVerts[i] += _groundPosition;            
           
            foreach (Vector2 pos in pathVerts)
            {
                Vertices localVerts = PolygonTools.CreateCircle(0.1f, 20);
                PolygonShape circleShape = new PolygonShape(localVerts, 1);
                Body localBody = new Body(_world);
                localBody.CreateFixture(circleShape);
                localBody.IsSensor = true;
                localBody.Position = pos;                
            }
           
            _body = new MyPlayerBody(_world);
            _body2 = new CpuBody(_world, pathVerts);

            _body.LoadContent();
            _body2.LoadContent();
        }

        private void moveVector(ref Vector2 vec)
        {
            vec += _groundPosition;
        }
        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            HandleGamePad();
            HandleKeyboard();

            _body.HandleKeyboard();
            _body2.HandleKeyboard();

            //narazie na sztywno do ciała 1
            _cameraPosition = -_body._body.Position * MeterInPixels + _screenCenter;

            //We update the world
            _world.Step((float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.001f);

            //killOrthogonalVelocity(_bodyTail, 1);
            //updateFriction(_body);
            //updateFriction(_bodyTail);                     
            _body.Update();
            _body2.Update();
            base.Update(gameTime);
        }

        private void killOrthogonalVelocity(Body body, float percent = 1)
        {
            Vector2 localPoint = new Vector2(0, 0);
            Vector2 velocity = body.GetLinearVelocityFromLocalPoint(localPoint);

            Transform tmp;
            body.GetTransform(out tmp);
            Vector2 sidewaysAxis = tmp.R.Col2;
            
            sidewaysAxis = percent * Vector2.Multiply(sidewaysAxis, Vector2.Dot(velocity, sidewaysAxis));
            body.LinearVelocity = sidewaysAxis;
        }

        private Vector2 getLateralVelocity(Body body)
        {
            Vector2 currentRightNormal = body.GetWorldVector(new Vector2(1, 0));
            return new Vector2(Vector2.Dot(currentRightNormal, body.GetLinearVelocityFromLocalPoint(new Vector2(0, 0)) * currentRightNormal));            
        }

        private Vector2 getForwardVelocity(Body body)
        {
            Vector2 currentRightNormal = body.GetWorldVector(new Vector2(0, 1));
            return new Vector2(Vector2.Dot(currentRightNormal, body.GetLinearVelocityFromLocalPoint(new Vector2(0, 0)) * currentRightNormal));   
        }

        void updateFriction(Body body)
        {
            Vector2 impulse = body.Mass * -getLateralVelocity(body);
            body.ApplyLinearImpulse(impulse, body.WorldCenter);
            body.ApplyAngularImpulse(0.1f * body.Inertia * -body.AngularVelocity);
        }

        private void HandleGamePad()
        {
            GamePadState padState = GamePad.GetState(0);

            if (padState.IsConnected)
            {
                if (padState.Buttons.Back == ButtonState.Pressed)
                    Exit();

                if (padState.Buttons.A == ButtonState.Pressed && _oldPadState.Buttons.A == ButtonState.Released)
                    _circleBody.ApplyLinearImpulse(new Vector2(0, -10));

                _circleBody.ApplyForce(padState.ThumbSticks.Left);
                _cameraPosition.X -= padState.ThumbSticks.Right.X;
                _cameraPosition.Y += padState.ThumbSticks.Right.Y;

                _view = Matrix.CreateTranslation(new Vector3(_cameraPosition - _screenCenter, 0f)) * Matrix.CreateTranslation(new Vector3(_screenCenter, 0f));

                _oldPadState = padState;
            }
        }

        private void HandleKeyboard()
        {
            KeyboardState state = Keyboard.GetState();
            const float kCameraMovement = 10.0f;
            // Switch between circle body and camera control
            if (state.IsKeyDown(Keys.LeftShift) || state.IsKeyDown(Keys.RightShift))
            {
                // Move camera
                if (state.IsKeyDown(Keys.A))
                    _cameraPosition.X += kCameraMovement;

                if (state.IsKeyDown(Keys.D))
                    _cameraPosition.X -= kCameraMovement;

                if (state.IsKeyDown(Keys.W))
                    _cameraPosition.Y += kCameraMovement;

                if (state.IsKeyDown(Keys.S))
                    _cameraPosition.Y -= kCameraMovement;

                _view = Matrix.CreateTranslation(new Vector3(_cameraPosition - _screenCenter, 0f)) *
                        Matrix.CreateTranslation(new Vector3(_screenCenter, 0f));
            }

            if (state.IsKeyDown(Keys.Escape))
                Exit();
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            string txt;
            GraphicsDevice.Clear(Color.CornflowerBlue);

            /* Circle position and rotation */
            // Convert physics position (meters) to screen coordinates (pixels)
            Vector2 circlePos = _circleBody.Position * MeterInPixels;
            float circleRotation = _circleBody.Rotation;

            /* Ground position and origin */
            Vector2 groundPos = _groundBody.Position * MeterInPixels;
            Vector2 groundOrigin = new Vector2(_groundSprite.Width / 2f, _groundSprite.Height / 2f);

            // Align sprite center to body position
            Vector2 circleOrigin = new Vector2(_circleSprite.Width / 2f, _circleSprite.Height / 2f);

            _batch.Begin(SpriteSortMode.Deferred, null, null, null, null, null, _view);

            //Draw circle
          // _batch.Draw(_circleSprite, circlePos, null, Color.White, circleRotation, circleOrigin, 1f, SpriteEffects.None, 0f);

            //Draw ground
           // _batch.Draw(_groundSprite, groundPos, null, Color.White, 0f, groundOrigin, 1f, SpriteEffects.None, 0f);

            _batch.End();

            _batch.Begin();

            // Display instructions
            _batch.DrawString(_font, Text, new Vector2(14f, 14f), Color.Gray);
            _batch.DrawString(_font, Text, new Vector2(12f, 12f), Color.White);
            txt = "body position = " + _body._body.Position.ToString();
            _batch.DrawString(_font, txt, new Vector2(12f, 80f), Color.Gray);
            _batch.DrawString(_font, txt, new Vector2(14f, 78f), Color.Wheat);
            /*
            txt = "_lateralvelocity = " + (getLateralVelocity(_body).LengthSquared().ToString());
            _batch.DrawString(_font, txt, new Vector2(12f, 60f), Color.Gray);
            _batch.DrawString(_font, txt, new Vector2(14f, 58f), Color.Wheat);
             */
            txt = "_velocity = " + (_body._body.LinearVelocity.LengthSquared()).ToString();
            _batch.DrawString(_font, txt, new Vector2(12f, 100f), Color.Gray);
            _batch.DrawString(_font, txt, new Vector2(14f, 98f), Color.Wheat);

            txt = "_bodyType1 = " + _body._bodyType1.ToString();
            _batch.DrawString(_font, txt, new Vector2(12f, 120f), Color.Gray);
            _batch.DrawString(_font, txt, new Vector2(14f, 118f), Color.Wheat);

            txt = "_bodyType2 = " + _body._bodyType2.ToString();
            _batch.DrawString(_font, txt, new Vector2(12f, 140f), Color.Gray);
            _batch.DrawString(_font, txt, new Vector2(14f, 138f), Color.Wheat);
            
            _batch.End();

            // calculate the projection and view adjustments for the debug view
            Matrix projection = Matrix.CreateOrthographicOffCenter(0f, _graphics.GraphicsDevice.Viewport.Width / MeterInPixels,
                                                             _graphics.GraphicsDevice.Viewport.Height / MeterInPixels, 0f, 0f,
                                                             1f);
            Matrix view = Matrix.CreateTranslation(new Vector3((_cameraPosition / MeterInPixels) - (_screenCenter / MeterInPixels), 0f)) * Matrix.CreateTranslation(new Vector3((_screenCenter / MeterInPixels), 0f));
            // draw the debug view
            _debugView.RenderDebugData(ref projection, ref view);

            _body.Draw(_batch);
            _body2.Draw(_batch);

            base.Draw(gameTime);

           
        }
    }
}